#include "RSIM_INC.h"
#include "Display.h"

#include <cstdlib>
#include <iomanip>
#include <fstream>

using namespace std;
using namespace RSIM;

void setState(State& state){
        double q1[] = {1,2,3};        
        double q2[] = {0,0,-PI/6};
        double q3[] = {1,2,3};
        double quat1[4],quat2[4],quat3[4];
        Quaternion::calcQuaternionFromEulerAngles(q1[0],q1[1],q1[2],"Body123",quat1);
        Quaternion::calcQuaternionFromEulerAngles(q2[0],q2[1],q2[2],"Body123",quat2);                
        Quaternion::calcQuaternionFromEulerAngles(q3[0],q3[1],q3[2],"Body123",quat3);                
        
//         state.updOneQ(PI/6,0,QIndex(0));
        
        state.updQ(quat1,0);        // Q for the node 0.                
//         state.updQ(quat2,1);     // Q for node 1
        state.updQ(quat3,2); // Q for node 2
                
}

int main(){                

        double mass = 1.0;
        Mat3x3 I;
        I.Diagonal(1.0,2.0,3.0);        

        MultibodySystem MBS1,MBS2,MBS3, MBS4;
        RigidBodyNode *tmp1, *tmp2, *tmp3, *tmp4, *tmp5, *tmp6, *tmp7;
        
        tmp1 = MBS1.setRoot(RigidBody("Ball",MassProperties(mass, I),Transform(Vect3(-1.5,0,0)),Transform(Vect3(0,0,0))));        
        tmp2 = MBS1.addNodeTo(tmp1, RigidBody("Pin",MassProperties(mass, I),Transform(Vect3(-1.5,0,0)),Transform(Vect3(1.5,0,0))));
        tmp3 = MBS1.addNodeTo(tmp2, RigidBody("Ball",MassProperties(mass, I),Transform(Vect3(-3.0,0,0)),Transform(Vect3(1.5,0,0))));	
        MBS1.setTopologyMaps();
        State state1(MBS1);
        setState(state1);
        
        
        tmp1 = MBS2.setRoot(RigidBody("Ball",MassProperties(mass, I),Transform(Vect3(-1.5,0,0)),Transform(Vect3(5,0,0))));        
        tmp2 = MBS2.addNodeTo(tmp1, RigidBody("Pin",MassProperties(mass, I),Transform(Vect3(-1.5,0,0)),Transform(Vect3(1.5,0,0))));
        tmp3 = MBS2.addNodeTo(tmp2, RigidBody("Ball",MassProperties(mass, I),Transform(Vect3(-3.0,0,0)),Transform(Vect3(1.5,0,0))));    
        MBS2.setTopologyMaps();
        State state2(MBS2);
        setState(state2);
        
        SystemData SD;
        
        SD.addSystemData(&MBS1,&state1);
	SD.addSystemData(&MBS2,&state2);
                                                                
        SD.constrain("Ball",    Transform(Vect3(0,0,0)),SystemIndex(0),NodeIndex(2),
                                Transform(Vect3(0,0,0)), SystemIndex(1),NodeIndex(2));
                                
        SD.constrainToGround("Ball",    Transform(Vect3(0,0,0)),SystemIndex(0),NodeIndex(2),
                                        Transform(Vect3(3.0,0,0)));                                                

        SD.finalize();        
        
        SD.updSystemPosition();
        
//         Display display;
//                  
//         display.render(MBS1);
//         display.render(MBS2);

        
        SD.attemptSatisfyPositionLevelConstraints();
        
        Display display;
                 
        display.render(MBS1);
        display.render(MBS2);
        
//         display.render(MBS1);

        return 0;
}
